#include "driver/i2c_master.h"
#include "esp_log.h"

#define TAG "pca9557"
#define PCA9557_INPUT (0)
#define PCA9557_OUTPUT (1)
// #define PCA9557_INPUT (2)
#define PCA9557_CONFIG (3)
#define PCA9557_I2C_ADDRESS (0X19)

#define SET_BITS(_m, _s, _v) ((_v) ? (_m) | ((_s)) : (_m) & ~((_s)))

i2c_master_dev_handle_t dev_handle;

esp_err_t pca9557_write(uint8_t reg, uint8_t data)
{
    uint8_t send_buf[2] = {reg, data};
    return i2c_master_transmit(dev_handle, send_buf, 2, 1000);
}
esp_err_t pca9557_read(uint8_t reg, uint8_t *data)
{
    return i2c_master_transmit_receive(dev_handle, &reg, 1, data, 1, 1000);
}

void pca9557_init(void)
{
    int ret = ESP_OK;
    i2c_master_bus_handle_t i2c_handle;
    i2c_master_get_bus_handle(CONFIG_I2C_PORT, &i2c_handle);
    // config i2c port
    ret = i2c_master_probe(i2c_handle, PCA9557_I2C_ADDRESS, 50);
    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG, "no find PCA9557, address:0x%02X",PCA9557_I2C_ADDRESS);
        return;
    }
    i2c_device_config_t i2c_dev_conf = {
        .scl_speed_hz = CONFIG_I2C_SPEED_KHZ * 1000,
        .device_address = PCA9557_I2C_ADDRESS,
    };

    if (i2c_master_bus_add_device(i2c_handle, &i2c_dev_conf, &dev_handle) != ESP_OK)
    {
        ESP_LOGE(TAG, "mount PCA9557 fail");
        return;
    }
    ESP_LOGI(TAG, "mount PCA9557 success");
    // config pac9557 port
    pca9557_write(PCA9557_CONFIG, 0xF8); // 1111 1000
    pca9557_write(PCA9557_OUTPUT, 0x05); // 0000 0101
}

void pca9557_set(uint8_t io, uint8_t data)
{
    uint8_t state = 0;
    pca9557_read(PCA9557_OUTPUT, &state);
    data = data == 0 ? 0 : 1;
    pca9557_write(PCA9557_OUTPUT, SET_BITS(state, io, data)); // 0000 0101

    ESP_LOGI(TAG, "state: %d,set:%d", state, SET_BITS(state, io, data));
}
